#include "MotorController.h"
#include "CANBus.h"
#include <chrono>
#include <thread>
#include <iostream>
#include <cmath>
#include <cstring>

#include <linux/can.h>
#include <linux/can/raw.h> // 尽量包含，即使 SOL_CAN_RAW 未定义
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <fcntl.h>

#include <unistd.h>
#include <net/if.h>

// 错误码相关头文件（errno）
#include <errno.h>


namespace jetcar {

MotorController::MotorController(const Config &cfg)
  : cfg_(cfg),
    target_direction_(0),
    target_speed_(cfg.default_speed),
    current_speed_left_(0),
    current_speed_right_(0),
    running_(false) {}

MotorController::~MotorController() {
    stopAll();
}

// ============ 启动与停止线程 ============
bool MotorController::start() {
    if (running_) return true;
    running_ = true;

    // 启动前自检
    std::cout << "[MotorController] Starting self check...\n";
    if (!selfCheck(cfg_.motor_left_id)) {
        std::cerr << "[WARN] Left motor offline or no response.\n";
    } else {
        std::cout << "[OK] Left motor online.\n";
    }
    if (!selfCheck(cfg_.motor_right_id)) {
        std::cerr << "[WARN] Right motor offline or no response.\n";
    } else {
        std::cout << "[OK] Right motor online.\n";
    }

    control_thread_ = std::thread(&MotorController::controlLoop, this);
    heartbeat_thread_left_ = std::thread(&MotorController::heartbeatLoop, this,cfg_.motor_left_id);
    //heartbeat_thread_right_ = std::thread(&MotorController::heartbeatLoop, this,cfg_.motor_right_id);
    std::cout << "[MotorController] Started.\n";
    return true;
}

void MotorController::stopAll() {
    running_ = false;
    // 给线程留出退出时间（可选，若线程逻辑中已有延迟则可省略）
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    
    if (control_thread_.joinable()) control_thread_.join();
    if (heartbeat_thread_left_.joinable()) heartbeat_thread_left_.join();
    if (heartbeat_thread_right_.joinable()) heartbeat_thread_right_.join();
    sendMotorCommands(0, 0);
    std::cout << "[MotorController] Stopped.\n";
}

// ============ 自检函数 ============
bool MotorController::selfCheck(int motor_id) {
    // 第一步：先创建 CAN 套接字并绑定（提前进入监听状态）
    int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (sock < 0) return false;

    struct sockaddr_can addr;
    struct ifreq ifr;
    std::strncpy(ifr.ifr_name, cfg_.can_device.c_str(), IFNAMSIZ-1);
    if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) { // 加错误判断
        close(sock);
        return false;
    }
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) { // 加错误判断
        close(sock);
        return false;
    }

    // 第二步：设置套接字为非阻塞模式（避免 read 卡住，确保 300ms 计时有效）
    int flags = fcntl(sock, F_GETFL, 0);
    if (flags < 0 || fcntl(sock, F_SETFL, flags | O_NONBLOCK) < 0) {
        close(sock);
        return false;
    }

    // 第三步：设置 CAN 过滤规则（只接收 motor_id 对应的帧，避免无关数据干扰）
    struct can_filter filter;
    filter.can_id = static_cast<uint32_t>(motor_id); // 只接收目标电机 ID
    filter.can_mask = CAN_SFF_MASK; // 标准帧掩码（如果是扩展帧用 CAN_EFF_MASK）
    if (setsockopt(sock, SOL_CAN_BASE + 1, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
        close(sock);
        return false;
    }

    // 第四步：发送自检指令（此时套接字已就绪，能立即监听响应）
    std::string error_msg = "";
    uint8_t cmd[1] = {0x33};
    sendCANFrame(static_cast<uint32_t>(motor_id), cmd, 1, error_msg);

    // 第五步：读取响应帧（300ms 超时）
    struct can_frame frame;
    bool ok = false;
    auto start = std::chrono::steady_clock::now();

    while (std::chrono::steady_clock::now() - start < std::chrono::milliseconds(300)) {
        int nbytes = read(sock, &frame, sizeof(frame));
        if (nbytes > 0) { // 只处理有效数据帧
            // 验证是否是电机的响应（ID 已通过过滤确保，可省略 ID 判断）
            if (frame.data[0] == 0x33) { // 假设电机响应的第一个字节是 0x33（需确认电机协议）
                ok = true;
                break;
            }
        } else if (nbytes == -1 && errno != EAGAIN) { // 非“无数据”的错误
            break;
        }

        // 小延时，降低 CPU 占用（可选，非必须）
        std::this_thread::sleep_for(std::chrono::microseconds(100));
    }

    close(sock);
    return ok;
}

// ============ 外部控制接口 ============
void MotorController::setTarget(int direction, int speed) {
    target_direction_.store(direction);
    
    // 手动实现 clamp：限制 speed 在 [cfg_.min_speed, cfg_.max_speed] 范围内（C++11 兼容）
    int clamped_speed = speed;
    if (clamped_speed < cfg_.min_speed) {
        clamped_speed = cfg_.min_speed;
    } else if (clamped_speed > cfg_.max_speed) {
        clamped_speed = cfg_.max_speed;
    }
    
    target_speed_.store(clamped_speed);
}

void MotorController::stop() {
    target_direction_.store(0);
    target_speed_.store(0);
}

// ============ 主控制循环 ============
void MotorController::controlLoop() {
    using namespace std::chrono;
    const int interval_ms = std::max(5, cfg_.control_interval_ms);
    const double accel_per_ms = cfg_.accel_per_sec / 1000.0;

    while (running_) {
        int dir = target_direction_.load();
        int tgt = target_speed_.load();
        int desired_left = 0, desired_right = 0;

        // 新增转向灵敏度参数（可从配置文件读取，范围0.1~0.9，默认0.5）
        // 越小转向越平缓，越大转向越灵敏
        const float turn_sensitivity = 0.5f; 
        switch (dir) {
            case Direction::STOP: 
                desired_left = 0; 
                desired_right = 0;
                break;
            case Direction::FORWARD: // 前进：左右电机同向同速
                desired_left = -tgt;
                desired_right = tgt;
                break;
            case Direction::BACKWARD: // 后退：左右电机同向同速（负号表示反向）
                desired_left = tgt;
                desired_right = -tgt;
                break;
            case Direction::LEFT_FRONT: // 左转（前进左转）：左电机减速，右电机保持原速
                desired_left = static_cast<int>(-tgt * (1 - turn_sensitivity)); // 内侧减速
                desired_right = tgt; // 外侧保持原速
                break;
            case Direction::RIGHT_FRONT: // 右转（前进右转）：右电机减速，左电机保持原速
                desired_left = -tgt; // 外侧保持原速
                desired_right = static_cast<int>(tgt * (1 - turn_sensitivity)); // 内侧减速
                break;
            case Direction::LEFT_BACK: // 左后转
                desired_left = tgt;
                desired_right = -static_cast<int>(tgt * (1 - turn_sensitivity));
                break;
            case Direction::RIGHT_BACK: // 右后转
                desired_left = static_cast<int>(tgt * (1 - turn_sensitivity));
                desired_right = -tgt;
                break;
            case 500: // 可选：原地左转（左右反向同速）
                desired_left = -tgt;
                desired_right = tgt;
                break;
            case 600: // 可选：原地右转（左右反向同速）
                desired_left = tgt;
                desired_right = -tgt;
                break;
            default: // 停止
                desired_left = 0;
                desired_right = 0;
                break;
        }

        
        // 可选：限制最小速度（避免内侧电机速度为0导致转向卡顿）
        const int min_turn_speed = 20; // 最小转向速度（根据电机特性调整）
        if (dir == 3 && std::abs(desired_left) < min_turn_speed) {
            desired_left = desired_left/std::abs(desired_left) * min_turn_speed;
        }
        if (dir == 4 && std::abs(desired_right) < min_turn_speed) {
            desired_right = desired_right/std::abs(desired_right) * min_turn_speed;
        }

        int curL = current_speed_left_.load();
        int curR = current_speed_right_.load();
        double max_delta = accel_per_ms * interval_ms;

        auto ramp = [&](int cur, int desired)->int {
            double diff = desired - cur;
            if (std::abs(diff) <= max_delta) return desired;
            return static_cast<int>(cur + std::copysign(max_delta, diff));
        };

        int nextL = ramp(curL, desired_left);
        int nextR = ramp(curR, desired_right);

        current_speed_left_.store(nextL);
        current_speed_right_.store(nextR);

        sendMotorCommands(nextL, nextR);

        std::this_thread::sleep_for(milliseconds(interval_ms));
    }
}

// ============ 心跳线程 ============
void MotorController::heartbeatLoop(int canId) {
    using namespace std::chrono;
    const int interval = std::max(10, cfg_.heartbeat_interval_ms);
    uint8_t hb_payload[1] = {0x00};
    std::string error_msg = "";
    while (running_) {
        sendCANFrame(static_cast<uint32_t>(cfg_.motor_left_id), hb_payload, 1,error_msg);
        sendCANFrame(static_cast<uint32_t>(cfg_.motor_right_id), hb_payload, 1,error_msg);
        std::this_thread::sleep_for(milliseconds(interval));
    }
}

// ============ 核心控制命令 ============
void MotorController::sendMotorCommands(int left_rpm, int right_rpm) {
    encodeAndSend(cfg_.motor_left_id, left_rpm);
    encodeAndSend(cfg_.motor_right_id, right_rpm);
}

/**
 * 速度控制模式（0x02）
 * 数据格式:
 *   Byte0 = 0x02
 *   Byte1-Byte4 = int32_t(ERPM)，高字节在前（大端）
 *   ERPM = RPM × 极对数
 */
void MotorController::encodeAndSend(int motor_id, int rpm) {
    const int pole_pairs = 10; // ⚠️ 修改为你的电机实际极对数
    int erpm = rpm * pole_pairs;

    uint8_t data[8] = {0};
    data[0] = 0x02;
    data[1] = (uint8_t)((erpm >> 24) & 0xFF);
    data[2] = (uint8_t)((erpm >> 16) & 0xFF);
    data[3] = (uint8_t)((erpm >> 8) & 0xFF);
    data[4] = (uint8_t)(erpm & 0xFF);

    static int retry = 0;
    std::string error_msg = "";
    if (!sendCANFrame(static_cast<uint32_t>(motor_id), data, 5,error_msg)) {

        // static bool first_fail = true;
        // static std::chrono::steady_clock::time_point last_log_time;
        // const int INTERVAL_MS = 10*1000; // 每隔2秒提示一次
        // auto now = std::chrono::steady_clock::now();
        // auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_log_time).count();
        // if (first_fail) {
        //     std::cerr << "[MotorController] FIRST FAIL: Failed to send speed to ID=" << motor_id<<", " <<error_msg<< std::endl;
        //     first_fail = false;
        //     last_log_time = now;
        // } else if (elapsed >= INTERVAL_MS) {
        //     std::cerr << "[MotorController] STILL FAILING: ID=" << motor_id <<", "<< error_msg <<  std::endl;
        //     last_log_time = now;
        // }

        // 构造错误日志（限制长度，避免终端自动换行）
        std::string base_log = "[MotorController] ID=" + std::to_string(motor_id) + 
                               ", " + error_msg + " (retry=" + std::to_string(retry) + ")";
        // 限制日志最大长度为 80 字符（适配终端宽度，避免自动换行）
        //if (base_log.size() > 80) {
        //    base_log = base_log.substr(0, 77) + "...";  // 超长截断加省略号
        //}
        // 单行覆盖输出：\r 回到行首 + 补空格清除残留 + 强制刷新
        std::cerr << "\r" << base_log;// << std::string(20, ' ');
        std::cerr.flush();
        
        // 重试间隔（500ms），重试次数递减
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        retry++;  // 关键：每次重试后递减，避免无限循环
        retry %= 1000000;
    }
    
}

} // namespace jetcar
